using System;
using System.Collections.Generic;
using System.Text;
using DotNetMatrix;
using System.Drawing;

namespace Blimpbots.Recognition
{
    public class Geometry
    {
        public static Point2D ProjectPoint(double x, double y, GeneralMatrix H)
        {
            GeneralMatrix k_Mat = Point2D.ToMatrix(x, y);
            GeneralMatrix projected = H * k_Mat;
            projected.HomogeneousNormalizeEquals();
            return new Point2D(projected.Array[0][0], projected.Array[1][0]);
        }
    }


    [Serializable]
    public struct Point3D
    {
        public double X;
        public double Y;
        public double Z;
        public Point3D(double X, double Y, double Z)
        { this.X = X; this.Y = Y; this.Z = Z; }

        /// <summary>
        /// Converts this 3D point to a matrix 
        /// </summary>
        /// <returns>3D homogeneous column-vector</returns>
        public GeneralMatrix ToMatrix()
        {
            return new GeneralMatrix(new double[] { X, Y, Z, 1.0 }, 4);
        }

        public double Distance(Point3D b)
        {
            return Math.Sqrt(Math.Pow(X - b.X, 2) +
                             Math.Pow(Y - b.Y, 2) +
                             Math.Pow(Z - b.Z, 2));
        }

        public static Point3D FromMatrix(GeneralMatrix matrix)
        {
            return new Point3D(
                   matrix.Array[0][0],
                   matrix.Array[1][0],
                   matrix.Array[2][0]);
        }

        public override string ToString()
        {
            return string.Format("[{0:f2},{1:f2},{2:f2}]", X, Y, Z);
        }
    }
}
